function d = point_to_point(point1, point2)
d = norm(point1(1:3) - point2(1:3));